%% Clear memory
close all
clc
clear

%% Parameters

% Physical constants
Rs  = 7.9e-3;
Ld  = .23e-3;
Lq  = .56e-3;
phi = 104e-3;
rpm = 0; % Initial rotor speed
wm  = rpm/60*2*pi;
np  = 4;
wr  = wm*np/2; 
J   = 0.1; % rotor inertia kgm^2


% Model of phiq & iq
x_pixels = [207 317 427 537 647 755 865 975 1085]; %Curve points
y_pixels = [617 587 543 491 426 358 304 261 232]; % Curve points
ki = (200--200)/(1085-207); % Iq/pixels
mi = 647; % location of Iq = 0 in pixels
kp = (0.1--0.1)/(165-686); % Phi/pixels
mp = 426; % location of Phiq = 0 in pixels
Iq = ki*(x_pixels-mi); % Est. Iq from pixel points.
Phiq = kp*(y_pixels-mp); % Est. Phiq from pixel points. 

% Model of phid & id
Id = linspace(-200,200,9);
Phid = Ld*Id + phi;

% Converter model
Udc = 100; 
fs  = 10000;
RD=0.0;     %Diode resistive voltage drop [Ohm]
RT=0.0;     %Transistor resistive voltage drop [Ohm]
ED=0.0;     %Diode on state constant voltage drop [V]
ET=0.0;     %Transistor on state constant voltage drop [V]

% Current controller
t_rc   = 2e-3;
a = log(9)/t_rc;
kp_d = a*Ld;
kp_q = a*Lq;
ki_d = a*a*Ld;
ki_q = a*a*Lq;
Rad  = a*Ld-Rs;
Raq  = a*Lq-Rs;

% Speed controller
w_ref = 6000;
t_rc = 1;
a    = log(9)/t_rc;
Ba   = a*J/phi;
kp_w = a*J/phi;
ki_w = 0.7*a*a*J/phi;
wmax = 6000;
Imax = 300;

% Load Torque
ext_torque = 0;

%% Simulation
ref_step_time_w = 0.1;
ref_step_time_t = 3;
sim('model_task4_no_conv',5,simset('Solver','ode45','MaxStep',1e-5));

%% Post-simulation evaluation
t       = pmsm.time - ref_step_time_w;
id      = pmsm.signals.values(:,1);
iq      = pmsm.signals.values(:,2);
theta   = pmsm.signals.values(:,3);
wm      = pmsm.signals.values(:,4);
psid    = pmsm.signals.values(:,5);
psiq    = pmsm.signals.values(:,6);
vd      = pmsm.signals.values(:,7);
vq      = pmsm.signals.values(:,8);
Dp      = current_controller.signals.values(:,1);
Di      = current_controller.signals.values(:,2);
Qp      = current_controller.signals.values(:,3);
Qi      = current_controller.signals.values(:,4);
id_ref  = current_controller.signals.values(:,5);
iq_ref  = current_controller.signals.values(:,6);
Wp      = speed_controller.signals.values(:,1);
Wi      = speed_controller.signals.values(:,2);
Wb      = speed_controller.signals.values(:,3);
vd_     = converter.signals.values(:,1);
vq_     = converter.signals.values(:,2);

%% Plotting
figure(1)
subplot(411)
plot(t, wm*60/(2*pi));hold on
legend('w_r [rpm]')
hold off
subplot(412)
plot(t, [id iq],'');hold on
legend('i_d[A]','i_q[A]')
hold off
subplot(413)
plot(t, [vd_ vq_],'');hold on
legend('v_d[V]','v_q[V]')
hold off
subplot(414)
plot(t, [vd_.*id vq_.*iq]/1000);hold on
legend('p_d[kW]','p_q[kW]')
hold off

%% Extra
figure(2)
%n = 1000;
%plot(t,filter(ones(1,n),n,vd))
%plot(t,[Dp Di Qp Qi]);legend('Dp','Di','Qp','Qi')
plot(t,[Wp Wi Wb]);legend('Wp','Wi','Wb')

